/****************************************************************************
*                                                                           *
*  OpenNI 1.x Alpha                                                         *
*  Copyright (C) 2011 PrimeSense Ltd.                                       *
*                                                                           *
*  This file is part of OpenNI.                                             *
*                                                                           *
*  OpenNI is free software: you can redistribute it and/or modify           *
*  it under the terms of the GNU Lesser General Public License as published *
*  by the Free Software Foundation, either version 3 of the License, or     *
*  (at your option) any later version.                                      *
*                                                                           *
*  OpenNI is distributed in the hope that it will be useful,                *
*  but WITHOUT ANY WARRANTY; without even the implied warranty of           *
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the             *
*  GNU Lesser General Public License for more details.                      *
*                                                                           *
*  You should have received a copy of the GNU Lesser General Public License *
*  along with OpenNI. If not, see <http://www.gnu.org/licenses/>.           *
*                                                                           *
****************************************************************************/
package com.robb19y.rgbdscanner.core;

import java.nio.ByteBuffer;
import java.util.Date;
import org.OpenNI.*;

public class RGBDSensor implements IObserver<ErrorStateEventArgs> , Runnable{
	/*OpenNI attributes*/
	private static final String SAMPLES_XML = "config/SamplesConfig.xml";	
	private OutArg<ScriptNode> scriptNodeArg;
	private Context context;
	private DepthGenerator depthGenerator;
	private DepthMetaData depthMD;
	private ImageGenerator imageGenerator;
	private ImageMetaData imageMD;
	private RGBDData [][]rgbdData;
	private RGBDData [][]rgbdDataBuffer;
	private int XRes;
	private int YRes;
	private double XResd;
	private double YResd;
	
	/*Parameters*/
	private long timeout = 300;
	private boolean running = false;	
	

	public RGBDSensor()  throws GeneralException{			
		
		//Initialize context
		scriptNodeArg = new OutArg<ScriptNode>();
		context = Context.createFromXmlFile(SAMPLES_XML, scriptNodeArg);		
		context.waitAnyUpdateAll();	
		System.out.println("Context initialized.");		
		
		//Initialize RGB sensor
		imageGenerator = (ImageGenerator) context.findExistingNode(NodeType.IMAGE);		
		imageMD = new ImageMetaData();		
		imageGenerator.getMetaData(imageMD);
		System.out.println("RGB sensor initialized. Resolution: "+imageMD.getXRes()+" x "+imageMD.getYRes()+".");
		XRes = imageMD.getXRes();
		YRes = imageMD.getYRes();
		XResd = (double) XRes	;	
		YResd = (double) YRes	;	
		
		//Initialize depth sensor
		depthGenerator = (DepthGenerator)context.findExistingNode(NodeType.DEPTH);
		
		depthGenerator.getAlternativeViewpointCapability().setViewpoint(imageGenerator);
		depthMD = new DepthMetaData();
		depthGenerator.getMetaData(depthMD);		
		System.out.println("Depth sensor initialized. Resolution: "+depthMD.getXRes()+" x "+depthMD.getYRes()+".");	
		
	}
	
	public void readRGBD() throws StatusException{		
		//Read sensor
		context.waitAnyUpdateAll();	
		depthGenerator.getMetaData(depthMD);
		imageGenerator.getMetaData(imageMD);
		rgbdDataBuffer = new RGBDData[depthMD.getXRes()/2][depthMD.getYRes()/2];		
		
		ByteBuffer imageBuffer = imageMD.getData().createByteBuffer();			
		
		double[] projection;
		double[] color;
		
		int i=0,r=0,g=0,b=0;		
		//Data acquisition
		for (int x = 0; x < imageMD.getXRes(); x = x + 2) {
			for (int y = 0; y < imageMD.getYRes(); y = y + 2) {
				i = y * imageMD.getXRes() + x;				
				//Get depth value					
				short z = depthMD.getData().readPixel(x, y);				
				
				//Get RGB values						
				r = imageBuffer.get(i * 3)  & 0xff; 
				g = imageBuffer.get(i * 3 + 1)  & 0xff; 
				b = imageBuffer.get(i * 3 + 2)  & 0xff;
				
				color = new double[]{r,g,b};
				projection = new double[]{x,y,z};			
				
				//Add RGBD point to collection							
				rgbdDataBuffer[x/2][y/2] = new RGBDData(color,projection);	
				rgbdDataBuffer[x/2][y/2].convert(XResd,YResd);
			}
		}	
		
		rgbdData = rgbdDataBuffer;
	}
	
	public double[] convertToRealWorld(double projection[]){		
		
		Point3D p3D = new Point3D((float) projection[0],(float) projection[1], (float) projection[2]);
		try{
			Point3D p3DReal = depthGenerator.convertProjectiveToRealWorld(p3D);
			double[] pointReal = {p3DReal.getX(),p3DReal.getY(),p3DReal.getZ()};
			pointReal[0] = pointReal[0]/1000;
			pointReal[1] = pointReal[1]/1000;
			pointReal[2] = pointReal[2]/1000;
			return pointReal;			
		} catch (StatusException e) {				
			e.printStackTrace();
		}
		return new double[]{0,0,-1};		
	}
	
	public static void main(String[] args) throws GeneralException, InterruptedException{
		RGBDSensor camera = new RGBDSensor();
		camera.start();
	}
	
	@Override
	public void update(IObservable<ErrorStateEventArgs> arg0, ErrorStateEventArgs arg1){
		System.out.printf("Global error state has changed: %s", arg1.getCurrentError());		
	}
	


	@Override
	public void run() {
		running = true;
		while(running){	
			Date d1 = new Date();
			try {
				readRGBD();
			} catch (Exception e) {				
				e.printStackTrace();
				System.out.println("Data acquisition failed.");
			}
			Date d2 = new Date();
			long delay = d2.getTime() - d1.getTime();
			System.out.println("Delay: "+delay+".");
			try {
				Thread.sleep(timeout);
			} catch (InterruptedException e) {				
				e.printStackTrace();
			}
		}		
	}
	
	public void start(){
		try {
			Thread.sleep(500);
		} catch (InterruptedException e) {			
			e.printStackTrace();
		}
		Thread task = new Thread(this);
		task.start();
	}
	
	public void stop(){
		running = false;
	}

	public RGBDData[][] getRgbdData() {
		return rgbdData;
	}

	public int getXRes() {
		return XRes;
	}

	public int getYRes() {
		return YRes;
	}

	public long getTimeout() {
		return timeout;
	}

	public void setTimeout(long timeout) {
		this.timeout = timeout;
	}	

	
}
